﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Collections;

namespace GeneralAdaptiveFramework
{
    public class PatrolDecide : PassiveComponent
    {
        readonly float moveSensitivivivity = 0.15f;
        readonly int failMessage = 2;

        int currentFail = 0;

        public string position = "";
        public float[] parameters = new float[4];
        public string message = "";

        public PatrolDecide(string _name)
            : base(_name)
        {
            initPort();
        }

        /**
         * Init two Provide ports and two Require ports
         */
        private void initPort()
        {
            interfaceList = new ArrayList();

            ProvideInterface provider01 = new ProvideInterface("PDecide01", this);
            ProvideInterface provider02 = new ProvideInterface("PDecide02", this);
            RequireInterface require01 = new RequireInterface("RDecide01", this);
            RequireInterface require02 = new RequireInterface("RDecide02", this);

            interfaceList.Add(provider01);
            interfaceList.Add(provider02);
            interfaceList.Add(require01);
            interfaceList.Add(require02);
        }

        /**
         * Receive data
         */
        public override void receiveData(Object data)
        {
            // add data to buffer
            buffer.Add(data);

            // call execute
            execute();
        }

        /**
         * Collect the object location by ColorDetection
         */
        public override void execute()
        {   
            // get data from buffer
            if (buffer.Count > 0)
            {
                ControlInfo controlInfo = new ControlInfo();
                // the data is an ObjectLocation
                position = ((string)buffer[0]);
                buffer.RemoveAt(0);
#if DEBUG
                Console.WriteLine(position);
#endif
                float[] parameters = makeParameters(position);
                controlInfo.parameters = parameters;
                controlInfo.message = message;
                
                // send data
                transferData(controlInfo);
            }
        }

        /**
         * Using Drone location to generate the parameter array
         */
        private float[] makeParameters(String position)
        {
            // init
            message = "";
            parameters[0] = 0; parameters[1] = 0; parameters[2] = 0; parameters[3] = 0;

            // execute
            if (position.Contains("LEFT"))
            {
                parameters[3] = -moveSensitivivivity;
                message += "Left ";
            }
            else if (position.Contains("RIGHT"))
            {
                parameters[3] = moveSensitivivivity;
                message += "Right ";
            }

            if (position.Contains("UP"))
            {
                parameters[2] = moveSensitivivivity;
                message += "Up ";
            }
            else if (position.Contains("DOWN"))
            {
                parameters[2] = -moveSensitivivivity;
                message += "Down ";
            }

            if (position.Contains("BACKWARD"))
            {
                parameters[1] = moveSensitivivivity;
                message += "Backward ";
            }
            else if (position.Contains("FORWARD"))
            {
                parameters[1] = -moveSensitivivivity;
                message += "Forward ";
            }

            /*
             * Special cases
             */
            if (position.Contains("EMPTY"))
            {
                ++currentFail;
                if (currentFail >= 2)
                {
                    // turn left
                    parameters[3] = -moveSensitivivivity;
                    message = "Left to find base";
                    currentFail = 0;
                }
            }
            else currentFail = 0;

            if (position.Contains("CENTER"))
            {
                // go forward
                parameters[1] = -moveSensitivivivity;
                message += "Forward to approach base";
            }

            if (position.Contains("ESCAPE"))
            {
                // go forward
                parameters[0] = 0;
                parameters[1] = 0;
                parameters[2] = 0;
                parameters[3] = 0;
                message += "ESCAPE due to low battery";
            }

            if (position.Contains("CLOSE"))
            {
                parameters[3] = -moveSensitivivivity;
                if (CollectForm.color == EZ_B.CameraDetection.ColorDetection.ColorEnum.Red)
                    CollectForm.color = EZ_B.CameraDetection.ColorDetection.ColorEnum.Blue;
                else
                    CollectForm.color = EZ_B.CameraDetection.ColorDetection.ColorEnum.Red;
                message += "Left to find the next base";
            }
            return parameters;
        }
    }
}
